#include "my_robot_hardware/zc4_hardware_interface.hpp"
#include <chrono>
#include <rclcpp/rclcpp.hpp>

namespace zc4_hardware {

hardware_interface::CallbackReturn Zc4HardwareInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  info_ = info;

  dxl_id_1 = 0;
  dxl_id_2 = 1;
  dxl_id_3 = 2;
  dxl_id_4 = 3;
  dxl_id_5 = 4;
  port_ = "/dev/ttyUSB0";

  driver_ = std::make_shared<XM430Driver>(port_);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_configure(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  if (driver_->init() != 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }
// +++ 添加这一行，初始化批量读写 +++
  if (!driver_->initBulkSync(dxl_id_1, dxl_id_2, dxl_id_3, dxl_id_4, dxl_id_5)) {
      std::cout << "Failed to initialize Bulk/Sync handlers!" << std::endl;
      return hardware_interface::CallbackReturn::ERROR;
  }

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_activate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  set_state("joint11/position", 0.0);
  set_state("joint2/position", 0.0);
  set_state("joint3/position", 0.0);
  set_state("joint4/position", 0.0);
  set_state("joint5/position", 0.0);
  driver_->activateWithPositionMode(dxl_id_1);
  driver_->activateWithPositionMode(dxl_id_2);
  driver_->activateWithPositionMode(dxl_id_3);
  driver_->activateWithPositionMode(dxl_id_4);
  driver_->activateWithPositionMode(dxl_id_5);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  driver_->deactivate(dxl_id_1);
  driver_->deactivate(dxl_id_2);
  driver_->deactivate(dxl_id_3);
  driver_->deactivate(dxl_id_4);
  driver_->deactivate(dxl_id_5);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type Zc4HardwareInterface::read(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  (void)period; // 添加(void)period; 避免编译器警告

    double joint11_pos = 0.0;
    double joint2_pos = 0.0;
    double joint3_pos = 0.0;
    double joint4_pos = 0.0;
    double joint5_pos = 0.0;
    // auto start = std::chrono::steady_clock::now();
    // 使用新的批量读函数
    if (driver_->readPresentPositions(dxl_id_1, joint11_pos, dxl_id_2, joint2_pos, dxl_id_3, joint3_pos, dxl_id_4, joint4_pos, dxl_id_5, joint5_pos))
    {
      set_state("joint11/position", joint11_pos+1.57);
      set_state("joint2/position", joint2_pos+0.55);
      set_state("joint3/position", -(joint3_pos+0.55));//转向也反了，这个地方就改成了+与getcommand同号
      set_state("joint4/position", joint4_pos-0.3);
      set_state("joint5/position", joint5_pos);
    }
  // auto end = std::chrono::steady_clock::now();
  // double dt_ms = std::chrono::duration<double, std::milli>(end - start).count();
  // RCLCPP_INFO(rclcpp::get_logger("Zc4HardwareInterface"),
  //             "read() took %.3f ms", dt_ms);

  return hardware_interface::return_type::OK;
}

#include <cmath>  // for std::isnan

hardware_interface::return_type Zc4HardwareInterface::write(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  (void)period;

  double c1 = get_command("joint11/position");
  double c2 = get_command("joint2/position");
  double c3 = get_command("joint3/position");
  double c4 = get_command("joint4/position");
  double c5 = get_command("joint5/position");

  // RCLCPP_INFO(
  //   rclcpp::get_logger("Zc4HardwareInterface"),
  //   "Commands: c1=%.3f, c2=%.3f, c3=%.3f, c4=%.3f, c5=%.3f",
  //   c1, c2, c3, c4, c5
  // );

  // 只要有一个是 NaN，就不写
  if (std::isnan(c1) || std::isnan(c2) || std::isnan(c3) || std::isnan(c4) || std::isnan(c5)) {
    // 不执行写、不报错，直接返回 OK
    return hardware_interface::return_type::OK;
  }

  // 正常写入
  driver_->writeTargetPositions(
    dxl_id_1, c1-1.57,
    dxl_id_2, c2-0.55,
    dxl_id_3, -(c3+0.55),
    dxl_id_4, c4+0.3,
    dxl_id_5, c5
  );
  return hardware_interface::return_type::OK;
}


}  // namespace zc4_hardware

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(zc4_hardware::Zc4HardwareInterface, hardware_interface::SystemInterface)